import { MathUtils, Ray, Sphere, Vector2, Vector3 } from "three";
import Geodetic3D from "./Geodetic3D";
import Config from "../Config";

export default class Ellipsoid {
    public center: Vector3;
    public radii: Vector3; //椭球的半径
    public radiiSquared: Vector3;
    public oneOverRadiiSquared: Vector3;
    public radiiToTheFourth: Vector3;
    public oneOverRadii: Vector3;


    public constructor(center?: Vector3, radii?: Vector3) {
        if (!center) {
            this.center = new Vector3(0, 0, 0);
        } else {
            this.center = center;
        }

        if (!radii) {
            this.radii = new Vector3(6378137.0, 6378137.0, 6356752.3142451793); //地球椭球的默认半径
        } else {
            this.radii = radii;
        }

        this.oneOverRadii = new Vector3(1 / this.radii.x, 1 / this.radii.y, 1 / this.radii.z);
        this.radiiSquared = new Vector3((this.radii.x * this.radii.x), (this.radii.y * this.radii.y), (this.radii.z * this.radii.z));
        this.oneOverRadiiSquared = new Vector3(1 / (this.radii.x * this.radii.x), 1 / (this.radii.y * this.radii.y), 1 / (this.radii.z * this.radii.z));
        this.radiiToTheFourth = new Vector3(
            this.radiiSquared.x * this.radiiSquared.x,
            this.radiiSquared.y * this.radiiSquared.y,
            this.radiiSquared.z * this.radiiSquared.z);
    }

    /**
     * 椭球上一个点的地心法线
     * @param positionOnEllipsoid 
     * @returns 
     */
    public static centricSurfaceNormal(positionOnEllipsoid: Vector3): Vector3 {
        return positionOnEllipsoid.clone().normalize();
    }

    /**
     * 椭球上一个点的大地表面法线
     * @param positionOnEllipsoid 
     * @returns 
     */
    public geodeticSurfaceNormal(positionOnEllipsoid: Vector3): Vector3 {
        return (positionOnEllipsoid.clone().multiply(this.oneOverRadiiSquared)).normalize();
    }

    /**
     * 把一个地理坐标中的三维点转换到ecef
     * @param geodeticPoint 
     * @returns 
     */
    public toEcef(geodeticPoint: Geodetic3D): Vector3 {
        let n: Vector3 = geodeticPoint.geodeticSurfaceNormal();
        let k: Vector3 = this.radiiSquared.clone().multiply(n);
        let gamma: number = Math.sqrt(
            (k.x * n.x) +
            (k.y * n.y) +
            (k.z * n.z));

        let rSurface: Vector3 = k.divideScalar(gamma);
        return rSurface.add(n.multiplyScalar(geodeticPoint.height));
    }

    /**把ecef坐标转换到地理坐标 */
    public toGeography(ecefPoint: Vector3): Geodetic3D {
        //返回新的Point(x, y, z, Const.EPSGType.EPSGWeb)
        var wgs84OneOverRadii = new Vector3(
            1.0 / this.radii.x,
            1.0 / this.radii.y,
            1.0 / this.radii.z
        );

        var wgs84CenterToleranceSquared = 0.1;
        var oneOverRadii = wgs84OneOverRadii;
        var oneOverRadiiSquared = this.oneOverRadiiSquared;
        var centerToleranceSquared = wgs84CenterToleranceSquared;

        var cartesian = new Vector3(ecefPoint.x, ecefPoint.y, ecefPoint.z);

        var p = this.scaleToGeodeticSurface(
            cartesian.clone(),
            oneOverRadii.clone(),
            oneOverRadiiSquared.clone(),
            centerToleranceSquared
        );

        if (!p) {
            return undefined;
        }
        let originPoint = p.clone();
        var n = p.multiply(oneOverRadiiSquared);
        n = n.normalize();

        var h = cartesian.clone().sub(originPoint);

        var longitude = MathUtils.radToDeg(Math.atan2(n.y, n.x));
        var latitude = MathUtils.radToDeg(Math.asin(n.z));

        var dotValue = h.dot(cartesian);
        if (dotValue > 0) {
            dotValue = 1;
        } else if (dotValue < 0) {
            dotValue = -1;
        } else if (dotValue == 0) {
            dotValue = 0;
        }
        var height = dotValue * h.length();

        return new Geodetic3D(longitude, latitude, height);
    }

    /**将ecef坐标点转换到大地坐标的表面 */
    public scaleToGeodeticSurface(
        cartesian: Vector3,
        oneOverRadii: Vector3 = this.oneOverRadii,
        oneOverRadiiSquared: Vector3 = this.oneOverRadiiSquared,
        centerToleranceSquared: number = 0.1
    ) {
        // 安全的参数检查
        if (!cartesian || !oneOverRadii || !oneOverRadiiSquared || centerToleranceSquared === undefined) {
            return undefined;
        }

        // 类型安全检查
        if (!(cartesian instanceof Vector3) || !(oneOverRadii instanceof Vector3) || !(oneOverRadiiSquared instanceof Vector3)) {
            return undefined;
        }

        // 归一化缩放因子 - 使用最大半径作为缩放基准以提高精度
        const scaleFactor = Math.max(this.radii.x, this.radii.y, this.radii.z);
        if (scaleFactor === 0) {
            return undefined;
        }

        // 在归一化空间中进行计算，减少大数值运算的精度损失
        const normalizedCartesian = cartesian.clone().divideScalar(scaleFactor);
        // 确保不修改传入的原始参数
        const normalizedOneOverRadii = oneOverRadii.clone().multiplyScalar(scaleFactor);
        const normalizedOneOverRadiiSquared = oneOverRadiiSquared.clone().multiplyScalar(scaleFactor * scaleFactor);
        const normalizedCenterToleranceSquared = centerToleranceSquared / (scaleFactor * scaleFactor);

        const positionX = normalizedCartesian.x;
        const positionY = normalizedCartesian.y;
        const positionZ = normalizedCartesian.z;

        const oneOverRadiiX = normalizedOneOverRadii.x;
        const oneOverRadiiY = normalizedOneOverRadii.y;
        const oneOverRadiiZ = normalizedOneOverRadii.z;

        const x2 = positionX * positionX * oneOverRadiiX * oneOverRadiiX;
        const y2 = positionY * positionY * oneOverRadiiY * oneOverRadiiY;
        const z2 = positionZ * positionZ * oneOverRadiiZ * oneOverRadiiZ;

        // 计算椭球的平方范数
        const squaredNorm = x2 + y2 + z2;
        const ratio = Math.sqrt(1.0 / squaredNorm);

        // 数值稳定性检查
        if (!isFinite(ratio)) {
            return undefined;
        }

        // 作为初始近似值，假设径向交点就是投影点
        const intersection = normalizedCartesian.clone().multiplyScalar(ratio);

        // 如果位置靠近中心，迭代将不会收敛
        if (squaredNorm < normalizedCenterToleranceSquared) {
            return intersection.clone().multiplyScalar(scaleFactor);
        }

        const oneOverRadiiSquaredX = normalizedOneOverRadiiSquared.x;
        const oneOverRadiiSquaredY = normalizedOneOverRadiiSquared.y;
        const oneOverRadiiSquaredZ = normalizedOneOverRadiiSquared.z;

        // 使用交点处的梯度代替真实的单位法线
        // 大小差异将被吸收到乘数中
        const gradient = new Vector3();
        gradient.x = intersection.x * oneOverRadiiSquaredX * 2.0;
        gradient.y = intersection.y * oneOverRadiiSquaredY * 2.0;
        gradient.z = intersection.z * oneOverRadiiSquaredZ * 2.0;

        // 计算法向量乘数lambda的初始猜测值
        const gradientLength = gradient.length();
        if (gradientLength < Number.EPSILON) {
            // 避免除以极小值
            return intersection.clone().multiplyScalar(scaleFactor);
        }

        let lambda = ((1.0 - ratio) * normalizedCartesian.length()) / (0.5 * gradientLength);
        let correction = 0.0;

        let func;
        let denominator;
        let xMultiplier;
        let yMultiplier;
        let zMultiplier;
        let xMultiplier2;
        let yMultiplier2;
        let zMultiplier2;
        let xMultiplier3;
        let yMultiplier3;
        let zMultiplier3;
        const EPSILON12 = 0.000000000001;

        // 迭代计数器，避免无限循环
        let iterationCount = 0;
        const maxIterations = 50;
        let converged = false;

        do {
            lambda -= correction;

            xMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredX);
            yMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredY);
            zMultiplier = 1.0 / (1.0 + lambda * oneOverRadiiSquaredZ);

            // 检查乘子的有效性
            if (!isFinite(xMultiplier) || !isFinite(yMultiplier) || !isFinite(zMultiplier)) {
                break;
            }

            xMultiplier2 = xMultiplier * xMultiplier;
            yMultiplier2 = yMultiplier * yMultiplier;
            zMultiplier2 = zMultiplier * zMultiplier;

            xMultiplier3 = xMultiplier2 * xMultiplier;
            yMultiplier3 = yMultiplier2 * yMultiplier;
            zMultiplier3 = zMultiplier2 * zMultiplier;

            func = x2 * xMultiplier2 + y2 * yMultiplier2 + z2 * zMultiplier2 - 1.0;

            // "denominator"（分母）在这里指的是该表达式在速度和加速度计算中的使用
            denominator =
                x2 * xMultiplier3 * oneOverRadiiSquaredX +
                y2 * yMultiplier3 * oneOverRadiiSquaredY +
                z2 * zMultiplier3 * oneOverRadiiSquaredZ;

            const derivative = -2.0 * denominator;

            // 添加导数检查，避免除以极小值导致的数值不稳定
            if (Math.abs(derivative) < EPSILON12) {
                break;
            }

            correction = func / derivative;
            iterationCount++;

            // 检查是否收敛
            if (Math.abs(func) <= EPSILON12) {
                converged = true;
                break;
            }
        } while (iterationCount < maxIterations);

        // 如果迭代未收敛，返回原始估计值或初始投影
        if (!converged) {
            // 如果函数值仍然很大，可能表明数值问题，使用初始估计
            if (Math.abs(func) > 1.0e3) {
                return intersection.clone().multiplyScalar(scaleFactor);
            }
            // 否则继续使用最后一次迭代结果，但请注意这是一个近似解
        }

        // 将结果转换回原始坐标系
        const result = new Vector3(
            positionX * xMultiplier * scaleFactor,
            positionY * yMultiplier * scaleFactor,
            positionZ * zMultiplier * scaleFactor
        );

        // 验证结果的有效性
        if (!isFinite(result.x) || !isFinite(result.y) || !isFinite(result.z)) {
            return undefined;
        }

        return result;
    }

    /**将ecef坐标点转换到地心坐标的表面 */
    public scaleToGeocentricSurface(position: Vector3): Vector3 {
        // 参数检查
        if (!position) {
            return undefined;
        }

        // 类型安全检查
        if (!(position instanceof Vector3)) {
            return undefined;
        }

        // 归一化缩放因子 - 使用最大半径作为缩放基准以提高精度
        const scaleFactor = Math.max(this.radii.x, this.radii.y, this.radii.z);
        if (scaleFactor === 0) {
            return undefined; // 与scaleToGeodeticSurface保持一致的返回值处理
        }

        // 在归一化空间中进行计算，减少大数值运算的精度损失
        const normalizedPosition = position.clone().divideScalar(scaleFactor);
        // 直接计算归一化的1/半径平方，避免潜在的精度问题
        const normalizedRadii = this.radii.clone().divideScalar(scaleFactor);
        const normalizedOneOverRadiiSquared = new Vector3(
            1.0 / (normalizedRadii.x * normalizedRadii.x),
            1.0 / (normalizedRadii.y * normalizedRadii.y),
            1.0 / (normalizedRadii.z * normalizedRadii.z)
        );

        // 计算平方和，避免重复计算
        const xSquared = normalizedPosition.x * normalizedPosition.x;
        const ySquared = normalizedPosition.y * normalizedPosition.y;
        const zSquared = normalizedPosition.z * normalizedPosition.z;

        // 使用更稳定的方式计算平方和，避免数值溢出
        let squaredSum = 0;
        const terms = [
            xSquared * normalizedOneOverRadiiSquared.x,
            ySquared * normalizedOneOverRadiiSquared.y,
            zSquared * normalizedOneOverRadiiSquared.z
        ];

        // 按绝对值从大到小排序，减少累加误差
        terms.sort((a, b) => Math.abs(b) - Math.abs(a));
        for (const term of terms) {
            squaredSum += term;
        }

        // 添加数值稳定性检查
        let beta;
        if (squaredSum < Number.EPSILON) {
            // 当输入点非常接近原点时，返回原点而非零向量，保持几何意义
            return new Vector3(0, 0, 0);
        } else {
            beta = 1.0 / Math.sqrt(squaredSum);
        }

        // 添加精度检查，避免极端情况下的数值问题
        if (!isFinite(beta)) {
            // 当beta不是有限数时，直接返回原点，与squaredSum接近0时的处理保持一致
            return new Vector3(0, 0, 0);
        }

        // 将结果转换回原始坐标系
        const result = normalizedPosition.clone().multiplyScalar(beta * scaleFactor);

        // 验证结果的有效性
        if (!isFinite(result.x) || !isFinite(result.y) || !isFinite(result.z)) {
            return undefined;
        }

        return result;
    }

    /**获得椭球体的半径 */
    public getRadii() {
        return this.radii.clone();
    }

    /**计算ecef椭球上两个点之间的曲线，注意： granularity参数必须为弧度；isPrecise如果为true生成的曲线会更符合地理测量学的意义，但是计算量会更大*/
    /**
     * 计算ecef椭球上两个点之间的曲线
     * @param startPoint 曲线的起点
     * @param endPoint 曲线的终点
     * @param granularityInRadian 用于生成曲线上各个点的弧度粒度(注意：必须为弧度，不能为角度)
     * @param isPrecise 为true生成的曲线会更符合地理测量学的意义，但是计算量会更大。默认为false，大多数情况下false就足够了
     * @param isShort 在startPoint和endPoint之间生成的是最近曲线，还是最远曲线。默认为false
     * @returns 返回曲线上的点数组
     */
    public computeCurve(startPoint: Vector3, endPoint: Vector3, granularityInRadian: number, isPrecise: boolean = false, isShort: boolean = true): Array<Vector3> {
        if (granularityInRadian <= 0) {
            throw new Error("粒度必须大于零。");
        }

        let startGeodeticPoint = this.toGeography(startPoint);
        let startHeight = startGeodeticPoint.height;

        let endGeodeticPoint = this.toGeography(endPoint);
        let endHeight = endGeodeticPoint.height;

        // let startPointLength = startPoint.clone().sub(this.center).length();
        let normal;
        let theta;
        if(isShort){
            normal = startPoint.clone().cross(endPoint).normalize();
            theta = startPoint.angleTo(endPoint);
        }else{
            normal = endPoint.clone().cross(startPoint).normalize();
            theta = Math.PI * 2 - startPoint.angleTo(endPoint);
        }
       
        let n = Math.max(Math.floor(theta / granularityInRadian) - 1, 0);

        let positions: Array<Vector3> = new Array<Vector3>();
        positions.push(startPoint.clone());

        for (let i = 1; i <= n; ++i) {
            let phi = (i * granularityInRadian);
            let rotatedPoint = startPoint.clone().applyAxisAngle(normal, phi);

            let progress = i / (n + 1);
            let pointHeight = startHeight + (endHeight - startHeight) * progress;

            let newPoint;
            if (!isPrecise) {
                newPoint = this.scaleToGeocentricSurface(rotatedPoint);
                newPoint = newPoint.clone().add(newPoint.clone().normalize().multiplyScalar(pointHeight));
            } else {
                newPoint = this.scaleToGeodeticSurface(rotatedPoint);
                let surfaceNormal = this.geodeticSurfaceNormal(newPoint);
                newPoint = newPoint.clone().add(surfaceNormal.multiplyScalar(pointHeight));
            }
            
            positions.push(newPoint);
        }

        positions.push(endPoint.clone());

        return positions;
    }

    public intersectRay(ray: Ray): Array<Vector3> {

        // 射线起点和方向
        const origin = ray.origin;
        const direction = ray.direction;

        // 归一化缩放因子 - 使用最大半径作为缩放基准
        const scaleFactor = Math.max(this.radii.x, this.radii.y, this.radii.z);

        // 避免除零错误
        if (scaleFactor === 0) {
            return null;
        }

        // 在归一化坐标系中计算
        const normalizedOrigin = origin.clone().divideScalar(scaleFactor);
        const normalizedDirection = direction.clone().normalize(); // 确保方向向量是单位向量

        // 归一化的椭球参数
        const normalizedRadii = this.radii.clone().divideScalar(scaleFactor);
        const normalizedOneOverRadiiSquared = new Vector3(
            1.0 / (normalizedRadii.x * normalizedRadii.x),
            1.0 / (normalizedRadii.y * normalizedRadii.y),
            1.0 / (normalizedRadii.z * normalizedRadii.z)
        );

        // 计算二次方程系数（在归一化空间）
        const A = normalizedDirection.x * normalizedDirection.x * normalizedOneOverRadiiSquared.x +
            normalizedDirection.y * normalizedDirection.y * normalizedOneOverRadiiSquared.y +
            normalizedDirection.z * normalizedDirection.z * normalizedOneOverRadiiSquared.z;

        // 添加小的 epsilon 避免除零错误
        const EPSILON = 1e-12; // 调整为更小的值，因为我们在归一化空间
        if (Math.abs(A) < EPSILON) {
            return null; // 射线方向几乎与椭球无交线
        }

        const B = 2 * (
            normalizedDirection.x * normalizedOrigin.x * normalizedOneOverRadiiSquared.x +
            normalizedDirection.y * normalizedOrigin.y * normalizedOneOverRadiiSquared.y +
            normalizedDirection.z * normalizedOrigin.z * normalizedOneOverRadiiSquared.z
        );

        const C = normalizedOrigin.x * normalizedOrigin.x * normalizedOneOverRadiiSquared.x +
            normalizedOrigin.y * normalizedOrigin.y * normalizedOneOverRadiiSquared.y +
            normalizedOrigin.z * normalizedOrigin.z * normalizedOneOverRadiiSquared.z - 1;

        // 计算判别式
        const discriminant = B * B - 4 * A * C;

        // 添加容错处理，避免由于浮点精度问题导致的误判
        const DISCRIMINANT_EPSILON = 1e-10;
        if (discriminant < -DISCRIMINANT_EPSILON) {
            return null; // 没有实根，射线与椭球不相交
        }

        // 对于接近零的判别式，视为有一个交点
        const adjustedDiscriminant = Math.max(discriminant, 0);
        const sqrtDiscriminant = Math.sqrt(adjustedDiscriminant);

        const t1 = (-B - sqrtDiscriminant) / (2 * A);
        const t2 = (-B + sqrtDiscriminant) / (2 * A);

        let tArray = [];
        if (t1 >= 0) {
            tArray.push(t1);
        }
        if (t2 >= 0 && Math.abs(t2 - t1) > 0.00001) {
            tArray.push(t2);
        }
        tArray.sort((a, b) => a - b);
        if (tArray.length == 0) {
            return null;
        }

        // 没有有效根
        if (tArray[0] === Infinity || tArray[0] > 1e10) { // 设置一个合理的上限，避免极端情况
            return null;
        }

        let intersectionArray = [];
        for (let t of tArray) {
            // 计算相交点（先在归一化空间，然后转换回原始空间）
            const normalizedIntersection = new Vector3(
                normalizedOrigin.x + normalizedDirection.x * t,
                normalizedOrigin.y + normalizedDirection.y * t,
                normalizedOrigin.z + normalizedDirection.z * t
            );

            // 将交点转换回原始坐标系
            const intersection = normalizedIntersection.multiplyScalar(scaleFactor);
            intersectionArray.push(intersection);
        }
        return intersectionArray;
    }

}